#include <lversion.h>
#if L_VERSION_MODULE_BV_TREE == 120118

#include "NormalCone.h"

using namespace llib;
using namespace llib::geom;

NormalCone NormalCone::Merge( const NormalCone& first, const NormalCone& second )
{
    NormalCone res( Vec3f(1.f,0.f,0.f), (Float)math::PI ); // invalid value initialization

    // merge operation only processed when both normal cone are valid
    if( !(first.Validate() && second.Validate()) )
        return res;

    // angle theta : the angle between two normal vector
    Float cosTheta = first._direction * second._direction;
    if(cosTheta > 1.f) cosTheta = 1.f; else if(cosTheta < -1.f) cosTheta = -1.f; // for numerical stability
    Float theta = acos(cosTheta);
    // angle delta : angle1 - angle2
    Float delta = first._angle - second._angle;
    if(delta < 0) delta = -delta;

    // check enclosure : merge result will be the larger one, when enclose happen
    if(theta < delta)
        return first._angle > second._angle ? first : second;

    // the REAL merge process
    Float A = first._direction * first._direction;
    Float B = first._direction * second._direction;
    Float C = second._direction * second._direction;
    Vec3f N2 = first._direction + second._direction;
    Float D = cos((first._angle - second._angle) / 2);
    D = (D * D) * (N2 * N2);
    Float E = B + C;
    Float F = A + B;
    Float P = E * E - C * D;
    Float Q = (E * F - B * D) * 2;
    Float R = F * F - A * D;
    
    // the equation P*x^2 + Q*x + R = 0 has prepared
    // we solve it here WITHOUT EquationSolver class
    // because delta is always non-negative,
    // and -Q == P * 2
    Float equationDelta = Q * Q - 4 * P * R;
    Float w;
    if(equationDelta > 0) {
        equationDelta = sqrt(equationDelta);
        if(P < 0) P = -P;
        if(first._angle > second._angle)
            w = 1.f - equationDelta / P / 2;
        else
            w = 1.f + equationDelta / P / 2;
    } else { // for numerical stability again (about 0.26%)
        // here delta == 0, which means:
        w = 1.f;
    }

    // prepare result
    res._angle = (theta + first._angle + second._angle) / 2;
    res._direction = first._direction + w * second._direction;
    res._direction.Normalize();

    return res;
}

#endif
